#include "Utils.h"
#include <iostream>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <ctime>
#include <cmath>
#include <sys/stat.h>

// 实现Utils命名空间中的函数

namespace Utils {

Utils::YMDTime getCurrentTime() {
    auto now = std::chrono::system_clock::now();
    std::time_t now_c = std::chrono::system_clock::to_time_t(now);
    std::tm* now_tm = std::localtime(&now_c);
    
    YMDTime result;
    result.year = now_tm->tm_year + 1900;
    result.month = now_tm->tm_mon + 1;
    result.day = now_tm->tm_mday;
    result.hour = now_tm->tm_hour;
    result.minute = now_tm->tm_min;
    result.second = now_tm->tm_sec;
    
    return result;
}

Utils::GPSTime ymdToGPSTime(const YMDTime& time) {
    // 将年月日时间转换为GPS时间
    // GPS时间起点: 1980-01-06 00:00:00
    
    // 首先转换为Julian日期
    int y = time.year;
    int m = time.month;
    int d = time.day;
    
    if (m <= 2) {
        y -= 1;
        m += 12;
    }
    
    int a = y / 100;
    int b = 2 - a + (a / 4);
    
    double jd = static_cast<int>(365.25 * (y + 4716)) + 
                static_cast<int>(30.6001 * (m + 1)) + 
                d + b - 1524.5;
    
    // 计算GPS日
    double gpsDay = jd - 2444244.5;
    
    // 计算GPS周和周内秒
    GPSTime gpsTime;
    gpsTime.week = static_cast<int>(gpsDay / 7.0);
    gpsTime.secondsOfWeek = (gpsDay - gpsTime.week * 7) * 86400.0 + 
                          time.hour * 3600.0 + 
                          time.minute * 60.0 + 
                          time.second;
    
    return gpsTime;
}

Utils::YMDTime gpsTimeToYMD(const GPSTime& gpsTime) {
    // 将GPS时间转换为年月日时间
    double totalDays = gpsTime.week * 7.0 + gpsTime.secondsOfWeek / 86400.0;
    double jd = totalDays + 2444244.5;
    
    // Julian日期转换为年月日
    int j = static_cast<int>(jd + 0.5);
    int g = static_cast<int>((j - 1867216.25) / 36524.25);
    j = j + 1 + g - static_cast<int>(g / 4);
    
    int a = j + 1524;
    int b = static_cast<int>((a - 122.1) / 365.25);
    int c = static_cast<int>(365.25 * b);
    int d = static_cast<int>((a - c) / 30.6001);
    
    YMDTime time;
    time.day = a - c - static_cast<int>(30.6001 * d);
    time.month = (d < 14) ? d - 1 : d - 13;
    time.year = (time.month > 2) ? b - 4716 : b - 4715;
    
    // 计算时分秒
    double dayFraction = (jd + 0.5) - static_cast<int>(jd + 0.5);
    double hoursTotal = dayFraction * 24.0;
    time.hour = static_cast<int>(hoursTotal);
    
    double minsTotal = (hoursTotal - time.hour) * 60.0;
    time.minute = static_cast<int>(minsTotal);
    
    time.second = (minsTotal - time.minute) * 60.0;
    
    return time;
}

double calculateMappingFunction(double elevation, double height, double lat, double azimuth) {
    constexpr double RE = 6371.0;  // 地球平均半径(km)
    
    // 转换为弧度
    double el = elevation * DEG2RAD;
    
    // 基本映射函数(单层模型)
    double z = PI/2.0 - el;
    double sinz = std::sin(z);
    double Re_h = RE / (RE + height);
    double chi = Re_h * sinz;
    double chi_sqr = chi * chi;

    // Modified single layer mapping function (Mannucci et al. 1999)
    if (chi_sqr >= 1.0) {
        return 100.0;  // Set a large value for low elevations
    }
    
    double mapping = 1.0 / std::sqrt(1.0 - chi_sqr);
    return mapping;
}

std::vector<double> calculateIonoPiercePoint(double lat, double lon, double el, double az, double height) {
    constexpr double RE = 6371.0;  // 地球平均半径(km)
    
    // 转换为弧度
    double lat_rad = lat * DEG2RAD;
    double lon_rad = lon * DEG2RAD;
    double az_rad = az * DEG2RAD;
    double el_rad = el * DEG2RAD;
    
    // 计算地心角
    double z = PI/2.0 - el_rad;
    double re_h = RE / (RE + height);
    double psi = z - asin(re_h * sin(z));
    
    // 计算穿刺点纬度
    double lat_ipp = asin(sin(lat_rad) * cos(psi) + cos(lat_rad) * sin(psi) * cos(az_rad));
    
    // 计算穿刺点经度
    double lon_ipp = lon_rad + atan2(sin(psi) * sin(az_rad), 
                                     cos(lat_rad) * cos(psi) - sin(lat_rad) * sin(psi) * cos(az_rad));
    
    // 转换为度并返回
    std::vector<double> ipp(2);
    ipp[0] = lat_ipp * RAD2DEG;
    ipp[1] = lon_ipp * RAD2DEG;
    
    // 标准化经度(-180到180度)
    if (ipp[1] > 180.0) ipp[1] -= 360.0;
    if (ipp[1] < -180.0) ipp[1] += 360.0;
    
    return ipp;
}

double timeDifference(const GPSTime& t1, const GPSTime& t2) {
    return (t2.week - t1.week) * 604800.0 + (t2.secondsOfWeek - t1.secondsOfWeek);
}

double normalizedLegendre(int n, int m, double x) {
    // 计算归一化的勒让德多项式 P_{nm}(x)
    if (m < 0 || m > n || std::abs(x) > 1.0) {
        return 0.0;
    }
    
    double pmm = 1.0;
    
    if (m > 0) {
        double somx2 = std::sqrt((1.0 - x) * (1.0 + x));
        double fact = 1.0;
        for (int i = 1; i <= m; i++) {
            pmm *= (-fact) * somx2;
            fact += 2.0;
        }
    }
    
    if (n == m) {
        return pmm;
    }
    
    double pmmp1 = x * (2 * m + 1) * pmm;
    if (n == m + 1) {
        return pmmp1;
    }
    
    double pnm = 0.0;
    for (int k = m + 2; k <= n; k++) {
        pnm = ((2 * k - 1) * x * pmmp1 - (k + m - 1) * pmm) / (k - m);
        pmm = pmmp1;
        pmmp1 = pnm;
    }
    
    return pnm;
}

bool saveVTECGridToCSV(const torch::Tensor& vtecGrid, const std::string& filename,
                            double latStart, double latStep,
                            double lonStart, double lonStep) {
    std::ofstream file(filename);
    if (!file.is_open()) {
        std::cerr << "无法创建文件: " << filename << std::endl;
        return false;
    }
    
    // 写入头部
    file << "Latitude,Longitude,VTEC" << std::endl;
    
    // 写入数据
    int numLat = vtecGrid.size(0);
    int numLon = vtecGrid.size(1);
    
    for (int i = 0; i < numLat; ++i) {
        double lat = latStart - i * latStep;
        
        for (int j = 0; j < numLon; ++j) {
            double lon = lonStart + j * lonStep;
            
            // 确保经度在-180到180之间
            if (lon > 180.0) lon -= 360.0;
            
            double vtec = vtecGrid[i][j].item<double>();
            
            file << std::fixed << std::setprecision(2) 
                << lat << "," << lon << "," 
                << std::setprecision(3) << vtec << std::endl;
        }
    }
    
    file.close();
    return true;
}

void printSystemInfo() {
    std::cout << "系统信息:" << std::endl;
    std::cout << "  - CPU架构: " << 
        #if defined(__x86_64__) || defined(_M_X64)
            "x86_64"
        #elif defined(__i386) || defined(_M_IX86)
            "x86"
        #elif defined(__arm__) || defined(_M_ARM)
            "ARM"
        #elif defined(__aarch64__)
            "ARM64"
        #else
            "Unknown"
        #endif
        << std::endl;
        
    std::cout << "  - 编译器: " << 
        #if defined(__GNUC__)
            "GCC " << __GNUC__ << "." << __GNUC_MINOR__ << "." << __GNUC_PATCHLEVEL__
        #elif defined(_MSC_VER)
            "MSVC " << _MSC_VER
        #elif defined(__clang__)
            "Clang " << __clang_major__ << "." << __clang_minor__ << "." << __clang_patchlevel__
        #else
            "Unknown"
        #endif
        << std::endl;
        
    std::cout << "  - Torch版本: " << TORCH_VERSION << std::endl;
    
    auto now = Utils::getCurrentTime();
    std::cout << "  - 当前时间: " 
              << now.year << "-" 
              << std::setfill('0') << std::setw(2) << now.month << "-" 
              << std::setfill('0') << std::setw(2) << now.day << " " 
              << std::setfill('0') << std::setw(2) << now.hour << ":" 
              << std::setfill('0') << std::setw(2) << now.minute << ":" 
              << std::setfill('0') << std::setw(2) << static_cast<int>(now.second) 
              << std::endl;
}

bool fileExists(const std::string& filename) {
    struct stat buffer;
    return (stat(filename.c_str(), &buffer) == 0);
}

bool createDirectory(const std::string& path) {
    #ifdef _WIN32
    return _mkdir(path.c_str()) == 0 || errno == EEXIST;
    #else
    return mkdir(path.c_str(), 0755) == 0 || errno == EEXIST;
    #endif
}

std::vector<std::string> split(const std::string& s, char delimiter) {
    std::vector<std::string> tokens;
    std::string token;
    std::istringstream tokenStream(s);
    while (std::getline(tokenStream, token, delimiter)) {
        tokens.push_back(token);
    }
    return tokens;
}

std::string trim(const std::string& s) {
    auto start = s.begin();
    while (start != s.end() && std::isspace(*start)) {
        start++;
    }
    
    auto end = s.end();
    do {
        end--;
    } while (std::distance(start, end) > 0 && std::isspace(*end));
    
    return std::string(start, end + 1);
}

std::vector<double> xyzToBlh(const std::vector<double>& xyz) {
    if (xyz.size() < 3) {
        return {0.0, 0.0, 0.0};
    }
    
    double x = xyz[0];
    double y = xyz[1];
    double z = xyz[2];
    
    double a = WGS84_A;
    double b = WGS84_B;
    double e2 = WGS84_E2;
    
    double p = std::sqrt(x*x + y*y);
    double theta = std::atan2(z*a, p*b);
    
    double lon = std::atan2(y, x);
    double lat = std::atan2(z + WGS84_EP2*b*std::pow(std::sin(theta), 3),
                          p - e2*a*std::pow(std::cos(theta), 3));
    
    double N = a / std::sqrt(1.0 - e2*std::pow(std::sin(lat), 2));
    double h = p / std::cos(lat) - N;
    
    // 转换为度
    lat = lat * RAD2DEG;
    lon = lon * RAD2DEG;
    
    // 确保经度在-180到180之间
    if (lon > 180.0) lon -= 360.0;
    if (lon < -180.0) lon += 360.0;
    
    return {lat, lon, h};
}

std::vector<double> blhToXyz(const std::vector<double>& blh) {
    constexpr double RE_WGS84 = 6378137.0;             // 长半轴(m)
    constexpr double FE_WGS84 = 1.0/298.257223563;     // 扁率
    constexpr double E2 = FE_WGS84*(2.0-FE_WGS84);     // 第一偏心率平方
    
    double lat = blh[0] * DEG2RAD;  // 纬度(弧度)
    double lon = blh[1] * DEG2RAD;  // 经度(弧度)
    double h = blh[2];              // 高度(m)
    
    double sin_lat = sin(lat);
    double cos_lat = cos(lat);
    double sin_lon = sin(lon);
    double cos_lon = cos(lon);
    
    // 计算卯酉圈曲率半径
    double N = RE_WGS84 / sqrt(1.0 - E2 * sin_lat * sin_lat);
    
    std::vector<double> xyz(3, 0.0);
    xyz[0] = (N + h) * cos_lat * cos_lon;
    xyz[1] = (N + h) * cos_lat * sin_lon;
    xyz[2] = (N * (1.0 - E2) + h) * sin_lat;
    
    return xyz;
}

std::vector<std::vector<double>> computeRotationMatrix(double lat, double lon) {
    double lat_rad = lat * DEG2RAD;
    double lon_rad = lon * DEG2RAD;
    
    std::vector<std::vector<double>> R(3, std::vector<double>(3, 0.0));
    
    // 计算旋转矩阵 R = R2(-π/2-λ) * R1(φ-π/2)
    R[0][0] = -std::sin(lon_rad);
    R[0][1] = std::cos(lon_rad);
    R[0][2] = 0.0;
    
    R[1][0] = -std::sin(lat_rad) * std::cos(lon_rad);
    R[1][1] = -std::sin(lat_rad) * std::sin(lon_rad);
    R[1][2] = std::cos(lat_rad);
    
    R[2][0] = std::cos(lat_rad) * std::cos(lon_rad);
    R[2][1] = std::cos(lat_rad) * std::sin(lon_rad);
    R[2][2] = std::sin(lat_rad);
    
    return R;
}

std::vector<double> computeENU(const std::vector<double>& refXyz, 
                                    const std::vector<double>& targetXyz,
                                    const std::vector<double>& refBlh) {
    if (refXyz.size() != 3 || targetXyz.size() != 3 || refBlh.size() != 3) {
        throw std::invalid_argument("坐标必须包含三个分量");
    }
    
    // 计算ECEF坐标差
    std::vector<double> delta(3);
    for (int i = 0; i < 3; i++) {
        delta[i] = targetXyz[i] - refXyz[i];
    }
    
    // 获取旋转矩阵
    auto R = computeRotationMatrix(refBlh[0], refBlh[1]);
    
    // 计算ENU坐标
    std::vector<double> enu(3, 0.0);
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            enu[i] += R[i][j] * delta[j];
        }
    }
    
    return enu;
}

// 年月日转GPS时间
GPSTime ymdhmsToGpst(int year, int month, int day, int hour, int minute, double second) {
    // Constants for GPS epoch (January 6, 1980)
    const int GPS_EPOCH_YEAR = 1980;
    const int GPS_EPOCH_MONTH = 1;
    const int GPS_EPOCH_DAY = 6;
    
    // Days per month (non-leap year)
    const int daysInMonth[] = {0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
    
    // Convert to days since GPS epoch
    int totalDays = 0;
    
    // Add days for years
    for (int y = GPS_EPOCH_YEAR; y < year; y++) {
        totalDays += 365;
        if ((y % 4 == 0 && y % 100 != 0) || y % 400 == 0) {
            totalDays++; // Leap year
        }
    }
    
    // Add days for months
    for (int m = 1; m < month; m++) {
        totalDays += daysInMonth[m];
        if (m == 2 && ((year % 4 == 0 && year % 100 != 0) || year % 400 == 0)) {
            totalDays++; // February in leap year
        }
    }
    
    // Add remaining days
    totalDays += day - GPS_EPOCH_DAY;
    
    // Convert to GPS weeks and seconds
    int week = totalDays / 7;
    double secondsOfWeek = (totalDays % 7) * 86400.0 + hour * 3600.0 + minute * 60.0 + second;
    
    return {week, secondsOfWeek};
}

// GPS时间转年月日
YMDTime gpstToYmdhms(const GPSTime& gpst) {
    const int mday[] = {0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
    YMDTime time;
    
    // GPS起始时间: 1980/1/6 00:00:00
    int mjd = gpst.week * 7 + (int)(gpst.secondsOfWeek / 86400.0);
    int days = mjd - 44244;  // 相对于1980/1/6的天数
    
    // 计算年份
    int year = 1980;
    while (days >= (year % 4 == 0 ? 366 : 365)) {
        days -= (year % 4 == 0 ? 366 : 365);
        year++;
    }
    
    // 计算月份
    int month = 1;
    int current_mday;
    while (1) {
        current_mday = mday[month];
        if (month == 2 && year % 4 == 0) current_mday++;
        if (days < current_mday) break;
        days -= current_mday;
        month++;
    }
    
    // 计算日期和时间
    time.year = year;
    time.month = month;
    time.day = days + 1;
    
    double tod = gpst.secondsOfWeek - (int)(gpst.secondsOfWeek / 86400.0) * 86400.0;  // 天内秒
    time.hour = (int)(tod / 3600.0);
    time.minute = (int)((tod - time.hour * 3600.0) / 60.0);
    time.second = tod - time.hour * 3600.0 - time.minute * 60.0;
    
    return time;
}

// GPS时间转BeiDou时间(BDT)
GPSTime gpstToBdt(const GPSTime& gpst) {
    GPSTime bdt = gpst;
    bdt.week = gpst.week - 1356;  // BDT从GPS周1356开始
    return bdt;
}

// BeiDou时间转GPS时间
GPSTime bdtToGpst(const GPSTime& bdt) {
    GPSTime gpst = bdt;
    gpst.week = bdt.week + 1356;
    return gpst;
}

// GPS时间转GLONASS时间
YMDTime gpstToGlot(const GPSTime& gpst) {
    GPSTime glot = gpst;
    glot.secondsOfWeek += 3 * 3600;
    return gpstToYmdhms(glot);
}

// 添加ecef2llh作为xyzToBlh的别名
std::vector<double> ecef2llh(const std::vector<double>& ecef) {
    return xyzToBlh(ecef);
}

} // namespace Utils 